COMMENT ⊗   VALID 00006 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	.TITLE DRIVE
C00006 00003	"CLKSER" - CLOCK INTERRUPT ROUTINE
C00011 00004	"ANGLES" - SUBR. TO READ CURRENT JOINT ANGLES AND HAND OPENING
C00014 00005	"DACOUT" - SUBR. TO OUTPUT DAC VALUES GIVEN JOINT ANGLES       
C00016 00006	"RELBRK"&"SETBRK" - SUBR. TO RELEASE AND SET BRAKE BITS        
C00018 ENDMK
C⊗;
.TITLE DRIVE

;THIS ROUTINE OUTPUTS THE DAC VALUES FOR A GIVEN JOINT MOTION.  "DRIVE"
;REQUIRES AS ITS ONLY ARGUMENT A POINTER TO A TABLE OF MOTION VARIABLES.
;A DESCRIPTION OF THE MOTION VARIABLE TABLE FOLLOWS:
;
;	DRVLST→	JTMASK	;BIT MASK TO CHECK IF ALL REQUIRED JOINTS ARE
;			;  WITHIN FINAL ERROR TOLERANCE.  THIS TEST
;			;  IS USED TO TERMINATE A NORMAL MOTION.
;		TTIME	;TOTAL PERMITTED JOINT MOTION TIME IN 5
;			;  MILLISECOND COUNTS.  A TIME OUT ERROR IS
;			;  AFTER TTIME*5 MILLISECONDS
;		RUNSUB	;ADDR. OF SUBR TO BE CALLED EVERY 20 MSEC TO
;			;  UPDATE THE JOINT SET POINTS. ALL REGISTERS
;			;  ARE AVAILABLE FOR USE, R0 ← SET PT. TIME,
;			;  R1 ← PTR TO JOINT ANGLE BUFFER. IF "STPMVE"
;			;  IS SET BY THIS ROUTINE R0 MUST CONTAIN AN
;			;  ERROR MESSAGE CODE ON EXIT.
;
;ALL JOINTS OF THE ARM JOINTS ARE SERVOED EVERY 5 MSEC. UNTIL EITHER THE
;MOTION IS COMPLETED,THE PANIC BUTTON IS HIT, OR A TIME OUT ERROR OCCURS.
;THE IN RANGE BITS FOR THE JOINTS THAT ARE BEING CHECKED ARE NOT USED UNTIL
;AFTER THE "FINAL" BIT IS SET IN THE 8TH WORD OF ONE OF THE JT ANGLE ARRAYS
;THAT ARE PASSED TO THE "RUNSUB" ROUTINE.  THE "RUNSUB" ROUTINE SHOULD SET
;THE "FINAL" BIT AFTER ALL INCREMENTAL SET POINTS HAVE BEEN COMPUTED.  IN
;BETWEEN CALLS TO "RUNSUB", LINEAR INTERPOLATION IS USED TO GENERATE
;INTERMEDIATE SET POINTS. A SAMPLE CALLING SEQUENCE TO "DRIVE" FOLLOWS:
;
;		MOV	#DRVLST,R1
;		JSR	PC,DRIVE
;
;"DRIVE" INDICATES ANY ERROR CONDITIONS GENERATED DURING THE MOTION
;BY LEAVING BITS SET IN THE ARM STATUS WORD "ARMS".

;REGISTERS USED:
;	R1 PASSES ARGUMENTS AND IS ALTERED
;	R0,R2,R3 ARE GARBAGED

;DEFINITIONS:

JTMASK	==0
TTIME	==2
RUNSUB	==4

DRIVE:	MOV	R1,@#DRVLST	;SAVE PTR TO DRIVE DATA
	MOV	@#MODES,R0	;SET ANY SPECIAL MODES REQUIRED
	JSR	PC,RELBRK
	MOV	#BUF1+2,R0	;INIT JT ANGLE BUFFERS TO CURRENT LOCATION
	MOV	#BUF2+2,R1
	MOV	#DANGLE,R2
	MOV	#7,R3
1$:	MOV	(R2),(R0)+
	MOV	(R2)+,(R1)+
	SOB	R3,1$
	CLR	(R0)		;CLEAR FINAL BIT
	CLR	(R1)
	MOV	#-1,@#PTIME	;PRESENT TIME IN 5 MSEC COUNTS
	CLR	@#DNECNT	;# OF SEQUENTIAL TIMES IN RANGE
	CLR	@#DRVERR	;ERROR MESSAGE CODE
	CLR	@#BADBIT	;JOINT OUT OF RANGE BITS
	BIS	#RUN,@#ARMS	;INDICATE ARM IN MOTION
AWAIT:	WAIT			;WAIT TILL MOTION COMPLETED
	BIT	#RUN,@#ARMS
	BNE	AWAIT
	MOV	@#DRVERR,R1	;ERROR MESSAGE INDEX
	BEQ	.+6
DRVMES:	JSR	PC,TYPERR	;TYPE OUT ANY NECESSARY ERROR MESS.
	MOV	@#MODES,R0	;TURN OFF SPECIAL MODES
	JSR	PC,SETBRK
	MOV	@#PMODES,@#MODES;CLEAR ALL TEMPORARY MODES
	RTS	PC

;END OF "DRIVE"
;"CLKSER" - CLOCK INTERRUPT ROUTINE

CLKSER:	MOV	R0,-(SP)	;SAVE ALL REGISTERS
	MOV	R1,-(SP)
	MOV	R2,-(SP)
	MOV	R3,-(SP)
	MOV	R4,-(SP)
	MOV	R5,-(SP)
	MOV	@#DR11S,-(SP)

	TST	@#WAITNG	;START-UP WAIT?
	BLE	1$
	DEC	@#WAITNG	;YES, TIME TO INITIALIZE RUNNING?
	BGT	SETDAC		;NO
	MOV	#ENABLE+ARMBRK,R0;YES, RELEASE ARM BRAKES
	JSR	PC,RELBRK

1$:	BIT	#RUN,@#ARMS	;ARM IN MOTION?
	BEQ	SETDAC		;NO JUST RE-FRESH DACS

	MOV	#STTMDE,@#DR11S	;FETCH SUBDEVICE BITS
	MOV	@#DR11I,R0
	MOV	@#DRVLST,R5	;GET POINTER TO DRIVE DATA

	MOV	#ABORT,R1	;HAS SOMEONE TYPED ON THE TTY?
	JSR	PC,TICKLE
	BCS	SIGERR

	BIT	#PANIC,R0	;PANIC BUTTON HIT?
	BEQ	SIGER1

	BIT	#ISON,R0	;HARDWARE SERVO STILL ENABLED?
	BEQ	SIGER2

	INC	@#PTIME		;INCREMENT TIME COUNT
	CMP	@#PTIME,TTIME(R5);OUT OF TIME?
	BLO	2$		;NO
	MOV	@#BADBIT,R1	;GET OUT OF RANGE BITS
	SWAB	R1
	BIS	#NOTIME,R1
	BR	SIGERR

2$:	BIT	#CHKDNE,@#ARMS	;WAITING FOR JOINTS TO FINISH?
	BEQ	NEWSET		;BRANCH IF STILL MOVING SET POINTS
	INC	@#DNECNT	;ASSUME JOINTS WITHIN FINAL RANGE
	MOV	JTMASK(R5),R2	;IN RANGE?
	BIC	R0,R2
	BEQ	3$		;YES
	CLR	@#DNECNT	;ELSE ZERO IN RANGE COUNT
	MOV	R2,@#BADBIT	;SAVE ERROR BITS
3$:	CMP	#6,@#DNECNT	;DON'T STOP TILL IN RANGE 6 TIMES
	BGT	SETDAC		;  IN A ROW
	BR	RUNDNE		;ALL DONE, STOP MOTION

NEWSET:	MOV	#DANGLE,R0	;UPDATE SET POINT
	MOV	#7,R2		;7 JOINTS IN ALL
	BIT	#3,@#PTIME	;TIME TO COMPUTE NEW JOINT ANGLES?
	BEQ	CALRUN		;YES
	MOV	#ONE4TH,R3	;NO, NEW ANG← OLD ANG + 1/4 CHANGE IN ANG
1$:	SUB	(R3)+,(R0)+
	SOB	R2,1$
	BR	SETDAC

CALRUN:	BIT	#WORKNG,@#ARMS	;RUNSUB CLOCK OVERRUN?
	BNE	SIGER3

	MOV	@CBUF,R1	;NEW ANGLE ← FUTURE ANGLE
	MOV	R1,@#CBUF
	MOV	(R1),R3		;NEW FUTURE ANGLE
	CMP	(R1)+,(R3)+
1$:	MOV	(R1)+,R4	;NEW SET POINT
	MOV	R4,(R0)+
	SUB	(R3)+,R4	;-CHANGE IN ANGLE/4
	ASH	#-2,R4
	MOV	R4,ONE4TH-DANGLE-2(R0)
	SOB	R2,1$

	BIT	#FINAL,(R1)	;FINAL SET POINT?
	BNE	ISFINL		;YES
	BIS	#STRWRK,@#ARMS	;ELSE EXECUTE RUNSUB
	BR	SETDAC

ISFINL:	MOV	#NOINTG,R0	;RE-ENABLE HARDWARE INTEGRATION
	JSR	PC,SETBRK
	BIS	#CHKDNE,@#ARMS	;INDICATE JUST NULLING ERRORS
	TST	JTMASK(R5)	;DO NULLING?
	BEQ	RUNDNE		;NO

SETDAC:	CLR	R0		;OUTPUT DAC VALUES
	MOV	#7,R1
	JSR	PC,DACOUT

	BIT	#STRWRK,@#ARMS	;START UP RUNSUB?
	BEQ	CLKDNE
	ADD	#STRWRK,@#ARMS	;INDICATE WORKING
	MOV	@#PTIME,R0	;TIME TO EVALUATE SET POINT
	ADD	#4,R0
	MOV	@#CBUF,R1	;POINT TO JOINT ANGLE BUFFER TO FILL
	TST	(R1)+
	MTPS	UNLOCK		;ALLOW CLOCK INTERRUPTS
	JSR	PC,@RUNSUB(R5)
	BIC	#WORKNG,@#ARMS
	BIT	#STPMVE,@#ARMS	;STOP ARM IMMEDIATELY?
	BEQ	CLKDNE		;NO
	BR	STPJT		;YES

SIGER3:	ADD	#RUNERR-NOHDWR,R1
SIGER2:	ADD	#NOHDWR-PANBUT,R1
SIGER1:	ADD	#PANBUT-ABORT,R1
SIGERR:	BIS	#CANPRO,@#ARMS	;SET ERROR BITS
	MOV	#-1,R0		;SET THE BRAKES
	JSR	PC,SETBRK
STPJT:	MOV	R1,@#DRVERR
RUNDNE:	BIC	#177400,@#ARMS

CLKDNE:	MOV	(SP)+,@#DR11S
	MOV	(SP)+,R5
	MOV	(SP)+,R4
	MOV	(SP)+,R3
	MOV	(SP)+,R2
	MOV	(SP)+,R1
	MOV	(SP)+,R0
	RTI


;END OF "CLKSER" ROUTINE
;"ANGLES" - SUBR. TO READ CURRENT JOINT ANGLES AND HAND OPENING

;JOINT ANGLES ARE READ FROM THE ADC AND CONVERTED INTO DEGREES AND
;INCHES.  THE FIRST JOINT TO BE READ MUST BE SPECIFIED BY REGISTER
;R0 ( I.E. 0-12 FOR THE ARM JOINTS AND 14 FOR THE HAND ) AND THE NUMBER
;OF JOINTS TO BE READ MUST BE SPECIFIED IN R1. A SAMPLE CALLING
;SEQUENCE FOLLOWS:
;
;		MOV	#FRSTJT,R0	;(FIRST JOINT - 1)*2
;		MOV	#JTS,R1		;NUMBER OF JOINTS TO BE READ
;		MOV	#JANGLES,R2	;SAVE READING IN HERE
;		JSR	PC,ANGLES	;NO ARGUMENTS REQUIRED
;
;IF THE ADC READ TAKES TOO LONG TO EXECUTE, THE C BIT IS SET, "ADERR"
;IS INDICATED IN THE ARM STATUS WORD, AND R1 CONTAINS AN ERROR CODE
;WHEN THIS ROUTINE EXITS.  OTHERWISE, THE C BIT IS CLEARED.

;REGISTERS USED:
;	R0,R1,R2  PASS ARGUMENTS AND ARE GARBAGED

ANGLES:	MOV	R4,-(SP)
	MOV	R5,-(SP)
	BIC	#3+ADCENB,@#DR11S	;SET DR11 TO ADC MODE
	BIS	#ADCMDE,@#DR11S	
	ADD	#ACHAN,R0		;SEND OFF FIRST POT CHANNEL
	MOV	(R0)+,@#DR11O
1$:	JSR	PC,ADCRDR		;R4←ADC READING
	BCS	ANGDNE			;ERROR?
	MOV	(R0)+,@#DR11O		;START CONVERTING NEXT POT
	MUL	SCALE-ACHAN-4(R0),R4	;CONVERT TO DEG. OR INCHES
	ASHC	#5,R4
	ADD	OFFSET-ACHAN-4(R0),R4
	MOV	R4,(R2)+
	SOB	R1,1$		;MORE POTS?
	CLC
ANGDNE:	MOV	(SP)+,R5
	MOV	(SP)+,R4
	RTS	PC


GETADC:	BIC	#3+ADCENB,@#DR11S	;ADC MODE
	BIS	#ADCMDE,@#DR11S
	MOV	R0,@#DR11O	;READ CHANNEL
	JSR	PC,ADCRDR
	RTS	PC


ADCRDR:	MOV	#100.,R4	;ADC TIME OUT COUNT
1$:	TSTB	@#DR11S		;ADC FINISHED?
	BMI	2$
	SOB	R4,1$		;LOOP TILL TIME RUNS OUT
	BIS	#CATERR,@#ARMS	;INDICATE ADC ERROR, CAN'T PROCEED
	MOV	#ADCERR,R1	;SIGNAL ERROR AND EXIT
	SEC
2$:	MOV	@#DR11I,R4	;ADC READING
	RTS	PC


;END OF "ANGLES"
;"DACOUT" - SUBR. TO OUTPUT DAC VALUES GIVEN JOINT ANGLES       

;THIS ROUTINE REQUIRES TWO ARGUMENTS, THE LOGICAL NUMBER OF THE FIRST
;JOINT TO HAVE IT'S DAC OUTPUT SET AND THE NUMBER OF JOINTS TO BE
;SERVICED.  ALL JOINTS FROM THE 1ST. JOINT TO (1ST JOINT -1 + NUMBER
;OF JOINTS) HAVE THEIR DAC VALUES SET.  THE DAC VALUES ARE TAKEN FROM
;THE ARRAY "DANGLE" AND ARE CONVERTED TO DAC UNITS VIA THE CONSTANTS
;BOFFST AND BSCALE.
;
;		MOV	#FRSTJT,R0	;(FIRST JOINT - 1)*2 
;		MOV	#JTS,R1		;# OF JOINTS TO BE SERVICED
;		JSR	PC,DACOUT	;NO ARGUMENTS REQUIRED
;
;THIS ROUTINE NEVER RETURNS AN ERROR CODE.

;REGISTERS USED:
;	R0 AND R1 PASS ARGUMENTS AND ARE GARBAGED
;	R2,R3 ARE GARBAGED

DACOUT:	BIC	#3,@#DR11S	;DR11 DAC MODE
DACO1:	MOV	DANGLE(R0),R2  	;DESIRED JOINT ANGLE
	SUB	BOFFST(R0),R2	;SUBT. POT OFFSET
	MUL	BSCALE(R0),R2	;CONVERT TO DAC UNITS
	ASHC	BSHIFT(R0),R2
	BIC	#170000,R2	;CLEAR ROOM FOR CHANNEL, POS. MODE
	BIS	DCHAN(R0),R2	;ADD DAC CHANNEL
	MOV	R2,@#DR11O   	;OUTPUT DAC READING AND CHANNEL
	TST	(R0)+
	SOB	R1,DACO1
	RTS	PC

;END OF "DACOUT"
;"RELBRK"&"SETBRK" - SUBR. TO RELEASE AND SET BRAKE BITS        

;THESE ROUTINES ARE USED TO SELECTIVELY TURN THE BITS IN THE BRAKE
;REGISTER ON AND OFF.  "RELBRK" SETS BITS AND "SETBRK" TURNS THEM
;OFF.  THE SOFTWARE REGISTER "BRAKES" IS USED TO KEEP TRACK OF
;THE STATE OF THE BITS IN BETWEEN CALLS TO THESE ROUTINES.  SAMPLE
;CALLING SEQUENCES TO THESE ROUTINES FOLLOWS:
;
;		MOV	#BRAKES,R0	;SEND BRAKE BITS
;		JSR	PC,RELBRK
;			or
;		JSR	PC,SETBRK

;REGISTERS USED:
;	R0 PASSES ARGUMENTS AND IS NOT MODIFIED

RELBRK:	MOV	#BRKMDE,@#DR11S	;SET BRAKE BIT MODE
	BIS	R0,@#BRAKES	;TURN ON OLD BITS + NEW BITS
	MOV	@#BRAKES,DR11O
	RTS	PC


SETBRK:	MOV	#BRKMDE,@#DR11S	;SET BRAKE BIT MODE
	BIC	R0,@#BRAKES	;TURN OFF THESE BITS
	MOV	@#BRAKES,@#DR11O
	RTS	PC
	

;END OF BRAKE ROUTINES